#ifndef _TEXLETFILTERTABLE_H_
#define _TEXLETFILTERTABLE_H_

#include <iostream>
#include <vector>
#include <string>
#include "TablePlane.h"

using namespace std;

struct InOut {
  vector<Texlet*> inliers;
  vector<Texlet*> outliers; 
};

struct PlaneEquation {
  double a,b,c,d;
  double nABC;
  bool estimated;
};

class TexletFilterTable {
 public:
  TexletFilterTable(double _sigma = 25.0, double _sigma2 = 25.0, double _angleT = 0.5, bool horizontalAssumption = false);
  ~TexletFilterTable();
  void setParameters(double _sigma, double _sigma2, double _angleT, bool _horizontalAssumption);
  InOut* filterInOut(vector<Texlet*> &texlets);
  void fitTablePlane(vector<Texlet*> &texlets);
  vector<Texlet*> filter(vector<Texlet*> &texletsIn);
  double getA() { return planeEquation.a; };
  double getB() { return planeEquation.b; };
  double getC() { return planeEquation.c; };
  double getD() { return planeEquation.d; };
  double getNABC() { return planeEquation.nABC; };
  vector<Texlet *> getTexletsTable() { return texletsTable; };

 private:
  bool includeUpward(double x, double y, double z, double n1, double n2, double n3);
  bool includeDownward(double x, double y, double z, double n1, double n2, double n3);
  bool includeUpward(double x, double y, double z);
  bool includeDownward(double x, double y, double z);

  int minNrRANSACIterations;
  int maxNrRANSACIterations;
  unsigned int minSamples;
  double sigma; // Texlets further from the table are excluded
  double sigma2; // Should be smaller than sigma. Above this threshold, texlets with a different 
                     // normal orientation than the plane estimation are excludes
  double angleT; // Angle threshold between texlet orientation and table plane orientation
  double R1;   // All texlets closer are not considered
  double R2;  // All texlets further are not considered
  bool horizontalAssumption;
  PlaneEquation planeEquation;
  vector<Texlet *> texletsTable;
};

#endif
